import numpy
import math
from dolfin import *

def directional_diameter_x(cell,v,x=None):
    '''Length of secant of triangle given by x and v. If x=None, barycenter of cell is used.'''
    if x is None:
        x = cell.midpoint()
        return directional_diameter_x(cell,v,x)
    else:
        if cell.intersects(x) == False:
            raise ValueError('Point x is not in the cell.')
        else:                              # now intersect is guaranteed
            center = cell.midpoint()
            v = v(center[0],center[1])     # direction at triangle is interpolated to barycenter, TODO direction as an argument?
        
            mesh = cell.mesh()
            vertices = mesh.coordinates()[cell.entities(0)] # grab vertices of intersected cell
        
            n = len(vertices)
            ps = []            # all intersects
            for i in range(n): # loop through edges |vertex1vertex2| and see if there is an intersect
                vertex1 = vertices[i % n]
                vertex2 = vertices[(i+1) % n]   
            
                A = numpy.array([ [vertex2[0]-vertex1[0],-v[0]],
                                  [vertex2[1]-vertex1[1],-v[1]] ])
                b = numpy.array([x[0]-vertex1[0],x[1]-vertex1[1]])
            
                try: # get t,s parameters and intersect
                    ts = numpy.linalg.solve(A,b)
                    p = numpy.array([vertex1[0] + ts[0]*(vertex2[0]-vertex1[0]),vertex1[1] + ts[0]*(vertex2[1]-vertex1[1])])
                except numpy.linalg.linalg.LinAlgError: # velocity parallel with edge
                    p = None

                if p is not None and (0 <= ts[0] <= 1): # check if the intesect of v-line and edge-line belongs to triangle
                    ps.append(p)
            
            n = len(ps)
            if n == 1:
                return 0
            elif n < 4 : # it would be to check if the new intersect is already in ps, but this is tricky due to round-off errors 
                norm = lambda a,b : math.sqrt((a[0]-b[0])**2 + (a[1]-b[1])**2)
                norms = []
                for i in range(n):
                    norms.append( norm(ps[i % n],ps[(i+1) % n]) )
                return numpy.amax(norms)
            else:
                raise ValueError('Weird number of intersects.')

def directional_diameter(cell,v):
    '''Length of the largest secant of triangle in direction v.'''
    mesh = cell.mesh()
    vertices = mesh.coordinates()[cell.entities(0)]   # grab vertices of intersected cell, it is enough just to check them and not
    return numpy.amax([directional_diameter_x(cell,v,Point(vertex[0],vertex[1])) for vertex in vertices]) # all points of triangle

def velocity_magnitude_x(cell,v,x=None):
    '''Velocity at cell as a magnitude of vector interpolated at x. If x is None, midpoint of cell is used.'''
    if x is None:
        x = cell.midpoint()
        return velocity_magnitude_x(cell,v,x)
    else:
        if cell.intersects(x) == False:
            raise ValueError('Point x is not in the cell.')
        else:
            v_interp = v(x[0],x[1]) 
            v_mag = math.sqrt(v_interp[0]**2 + v_interp[1]**2)
            return v_mag
    
def velocity_magnitude(cell,v):
    '''Velocity at cell as an equally-weighted(1./3.) sum of magnitudes of vectors at cells vertices.'''
    mesh = cell.mesh()
    vertices = mesh.coordinates()[cell.entities(0)] 
    
    v_mag = 0
    for vertex in vertices:
        v_vertex = v(vertex[0],vertex[1])
        v_mag += math.sqrt( v_vertex[0]**2 + v_vertex[1]**2 )
        
    v_mag = v_mag
    v_mag /= 3.
    return v_mag
